#include <QDataStream>
#include "RPiHeliTCPClient.h"
#include "rpi_sensors.h"
#include "rpi_control.h"

RPIHelicopterClient::RPIHelicopterClient(QObject *parent) :
    QTcpSocket (parent){
    //connect(this,SIGNAL(newConnection()),this,SLOT(connectionMade()));
    qDebug() << "Raspberry Pi Client Class Opened!";
    connect(this,SIGNAL(readyRead()),this,SLOT(receiveData()));
    //zeroTime = QDateTime::
    time =0;
    accelerometer = new rpi_sensor_acc(rpi_sensor_acc::SENSOR_MODE_CLIENT);
    gyroscope = new rpi_sensor_gyro(rpi_sensor_gyro::SENSOR_MODE_CLIENT);
    gps = new rpi_sensor_gps(rpi_sensor_gps::SENSOR_MODE_CLIENT);
    pressure = new rpi_sensor_pressure(rpi_sensor_pressure::SENSOR_MODE_CLIENT);
    compass = new rpi_sensor_compass(rpi_sensor_compass::SENSOR_MODE_CLIENT);
    imu =  new rpi_imu_9dof(rpi_imu_9dof::SENSOR_MODE_CLIENT,zeroTime);
}

RPIHelicopterClient::~RPIHelicopterClient() {
}


void RPIHelicopterClient::connectionMade() {
    qDebug() << "Connection Made";
    rpi_request r(RPI_SENSOR_ACC);
}

void RPIHelicopterClient::steerTo(int i) {
    sendInt(RPI_SERVO_STEER,i);
}

void RPIHelicopterClient::setMotorPower(int i) {
    sendInt(RPI_SERVO_MOTOR,i);
}

void RPIHelicopterClient::receiveData(){
    //qDebug() << "DataReadyToReceive";


    QDataStream in(this);
    in.setVersion(QDataStream::Qt_4_0);
    quint16 blockSize =0;
    while (bytesAvailable() > (int)sizeof(quint16)) {
     in >> blockSize;
     while (bytesAvailable() < blockSize) waitForBytesWritten();

    QString xml;
    in >> xml;
    //qDebug() << xml;

    QDomDocument xmlDoc;
    xmlDoc.setContent(xml);

    QDomElement docElem = xmlDoc.documentElement();
    qDebug() << "incoming XML " << docElem.tagName();
    if (docElem.tagName() == "Accelerometer") {
        accelerometer->readXML(xml);
        emit updateAccelerometer();
    }
    else if(docElem.tagName() == "Control") {

        rpi_control *c = new rpi_control;
        c->readDomElement(docElem);
        //qDebug() << xml;
        //qDebug() << c->toXML();

        qDebug() << "Received Control" << c->id();

        if (controls.size() == c->id()) {
            controls << c;
        }else if ( controls.size() > c->id() )
        {
            rpi_control *d = controls[c->id()];
            controls[c->id()] = c;
            delete d;
        }
        emit updateControls();
    }else if(docElem.tagName() == "imu9dof" ) {
        imu->readXML(xml);
        emit updateIMU(imu);
    }else if(docElem.tagName() == "GPS" ) {
        gps->readXML(xml);
        //qDebug() << xml;
        emit updateGPS();
    }

    }
}

void RPIHelicopterClient::sendFastControl(int c,int v){
   if (state() != QAbstractSocket::ConnectedState) {
    qDebug() << "Message Not Sent because of state" << state();
    return; }
    QByteArray block;
    QDataStream out(&block, QIODevice::WriteOnly);
    out.setVersion(QDataStream::Qt_4_0);

    out << (quint16)0;
    out << 0<< c << v;
    out.device()->seek(0);
    out << (quint16)(block.size() - sizeof(quint16));
    write(block);
    flush();

    qDebug() << "Fast Message";
}


void RPIHelicopterClient::sendXML(QString x) {
    if (state() != QAbstractSocket::ConnectedState) {
    qDebug() << "Message Not Sent because of state" << state();
    return; }
    QByteArray block;
    QDataStream out(&block, QIODevice::WriteOnly);
    out.setVersion(QDataStream::Qt_4_0);

    out << (quint16)0;
    qDebug() << "qunit" << block.size();
    
    out << x.toStdString().c_str();
    //qDebug() << x;
    
    out.device()->seek(0);
    out << (quint16)(block.size() - sizeof(quint16));
    write(block);
    flush();
    waitForBytesWritten(100);
    qDebug() << "Message XML Sent"<< block.size() << x.size() << x.left(3) << x.right(3) ;
    
}


void RPIHelicopterClient::sendInt(quint16 type,quint16 value) {
    if (state() != QAbstractSocket::ConnectedState) return;
    QByteArray block;
    QDataStream out(&block, QIODevice::WriteOnly);
    out.setVersion(QDataStream::Qt_4_0);

    out << (quint16)0;
    out << type << value;
    out.device()->seek(0);
    //out << (quint16)(block.size());// - sizeof(quint16));
    write(block);
    flush();
}
void RPIHelicopterClient::saveConfig(){

    rpi_request racc(rpi_request::REQUEST_SAVE_CONFIG);
    sendXML(racc.toXML());

}

void RPIHelicopterClient::pollDevices(){

    rpi_request racc(rpi_request::REQUEST_ACCELEROMETER);
    sendXML(racc.toXML());

    rpi_request rgyro(rpi_request::REQUEST_GYROSCOPE);
    sendXML(rgyro.toXML());

    rpi_request rcomp(rpi_request::REQUEST_COMPASS);
    sendXML(rcomp.toXML());

    rpi_request rgps(rpi_request::REQUEST_GPS);
    sendXML(rgps.toXML());

    rpi_request rpress(rpi_request::REQUEST_PRESSSURE);
    sendXML(rpress.toXML());

    rpi_request rultral(rpi_request::REQUEST_ULTRASONIC_LIST_SIZE);
    sendXML(rultral.toXML());

    rpi_request rcontrl(rpi_request::REQUEST_CONTROL_LIST_SIZE);
    sendXML(rcontrl.toXML());
}

void RPIHelicopterClient::requestControls() {
    rpi_request rcontrl(rpi_request::REQUEST_INSERT_CONTROL);
    sendXML(rcontrl.toXML());
}

void RPIHelicopterClient::sendControl(int k) {
    qDebug() << "sendControl" << k;
    if (k<controls.size()) sendXML(controls[k]->toXML());
}
